{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys; sys.path.append('..')\n",
    "from pb_diff_envs.environment.static.comp_rec_group import ComposedRandRecGrp \n",
    "import numpy as np\n",
    "from pb_diff_envs.environment.comp.comp_stdyn_m2d_wgrp import ComposedStDynRM2DGroupList\n",
    "from pb_diff_envs.robot.point2d_robot import Point2DRobot\n",
    "import gym\n",
    "\n",
    "# maze_size = np.array((5,5)); half_extents = np.array((0.5, 0.5)); gap_betw_wall = 0.15 # 0.10 too narrow\n",
    "# seed = 27; gen_data = True; robot_class = None\n",
    "# n_comp = 2; num_walls_c = np.array([6, 3])\n",
    "# half_extents_c = np.array([ [ 0.5, 0.5], [ 0.7, 0.7] ])\n",
    "\n",
    "\n",
    "env_name = 'CompDynrandSmaze2d-testOnly-v4'\n",
    "dvg_group = gym.make(env_name, load_mazeEnv=False, gen_data=True) # True False\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "env = dvg_group.create_single_env(4) # choose number 4th env\n",
    "## render a list of image showing the moving obstacle in temporal order\n",
    "img = env.render()\n",
    "from utils.maze2d_utils import plt_img\n",
    "plt_img(img, dpi=200)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "prev_pose = env.get_robot_free_pose() # np.array([0.0] * env.robot.config_dim)\n",
    "env.robot.set_config(prev_pose)\n",
    "solutions = []\n",
    "wtrajs_list = []\n",
    "### SIPP is slow, might takes minutes to plan\n",
    "### we treat the static obstalce as a moving obstacle whose moving trajectory always stays at the same position\n",
    "n_traj = 2\n",
    "for i in range(n_traj):\n",
    "    solution, d = env.sample_1_episode(prev_pose)\n",
    "    wtrajs = d['wtrajs'] # n_w, h, 2\n",
    "    wtrajs_list.append(wtrajs)\n",
    "\n",
    "    solution = np.array(solution)\n",
    "    prev_pose = solution[-1]\n",
    "    solutions.append(solution)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from glob import glob\n",
    "savepath = './test_sipp_comp_stdyn_maze2d.gif'\n",
    "sol_i = solutions[0]\n",
    "wtrajs_i = wtrajs_list[0]\n",
    "gifs = env.render_1_traj(sol_i, savepath=savepath, wtrajs=wtrajs_i)\n",
    "savepath = glob(f\"{savepath.replace('.gif','')}**\")[0] ## retrieve the gif path\n",
    "###\n",
    "print(len(gifs), gifs[0].shape)\n",
    "from IPython.display import HTML\n",
    "import base64\n",
    "\n",
    "b64 = base64.b64encode(open(savepath, 'rb').read()).decode('ascii')\n",
    "display(HTML(f'<img src=\"data:image/gif;base64,{b64}\" />'))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
